/*! \file swing_up_motor1.cpp

\brief Swing up controller for furuta pendulum system

Date of creation: 2010-15-11

Copyright
Julian David Colorado, jd.colorado@upm.es
Universidad Politecnica de Madrid

See the file "license.terms" for information on usage and redistribution of this file, and for a
DISCLAIMER OF ALL WARRANTIES.
*/
/*
INPUT
\param MAT SimParam: Simulation Parameters 4 x 6
\param VEC X: State vector 4
\param int i: Time flag
\param int ui: Real time

OUTPUT
\param double TAU: Torque input
\param VEC X: State derivate

*/
#include "../../dynlib.h"

void swing_up(Matrix &SimParam,Matrix &X,int i,int ui,double *Tau,ColumnVector &dX)
{
	double signo,temp,temp2,Eo,w,E;
	
	//Sign of control action
	if (X(4,i)* cos(X(2,i))>0.0)
		signo=1.0;
	else
		signo=-1.0;

	temp=1.0;
	Eo=10.0;

	//w=sqrt((m*g*L)/(4*I))
	w=sqrt((SimParam(1,1)*SimParam(3,5)*SimParam(1,4))/(4*SimParam(1,5)));
	
	temp2=(X(4,i)/w)*(X(4,i)/w);

	//E=m*g*L*(0.5*(dteta[1][i+1]/w)**2+cos(teta[1][i+1])-1)
	E=SimParam(1,1)*SimParam(3,5)*SimParam(1,4)*(0.5*temp2+cos(X(2,i))-1);
	
	//u=(Ta/Ka)*(m*const*(E-Eo)*signo+(1/Ta)*dteta[0][i+1])
	*Tau=((SimParam(2,2)/SimParam(2,1))*(SimParam(1,1)*temp*(E-Eo)*signo+(1/SimParam(2,2))*X(3,i)))*1/3;
		
}
